#include <MsTimer2.h>

#define DEBUG 1

#define EN_A 9 // Enable motor A
#define b1_A 10 //motor A bit1 
#define b2_A 11 //motor A bit2 
#define numero_voltas 200
#define Ta 50 // Tempo de amostragem (em milisegundos)
#define STEP_TIME 10 // Tempo de cada degrau (em segundos)
#define FRENTE 1
#define TRAS 0
#define PPR 112 // Pulses Per Revolution - CHANGE interrupt
//#define PPR 60 // Pulses Per Revolution - FALLING interrupt
#define tensaoMotor 8 // tensao de alimentacao dos motores

int n = 2; // you will understand why...
int k = 4; // you will understand why too...
long int contador[3] = {0,0,0};
long double tempo = 0;
int index = 1;

float ref = 0;
float velocidade[5] = {0,0,0,0,0};
float e[3] = {0,0,0}; // erro = ref - y
float u[3] = {0,0,0}; // sinal de controle
float yf; // saida no instante "n"

static boolean flag = LOW; 
static boolean changeRef = 0; // DEIXAR 0 !!!!!!!!!!!
int sentido_rot;


void setup()
{
  Serial.begin(9600);

  MsTimer2::set(Ta, speedControl);

  pinMode(EN_A,OUTPUT);
  pinMode(b1_A,OUTPUT);
  pinMode(b2_A,OUTPUT);

  pinMode(13, OUTPUT); // LED pin
  pinMode(2, INPUT); // interrupt 0

  // Default = FRENTE
  digitalWrite(b1_A, HIGH);
  digitalWrite(b2_A, LOW);
  sentido_rot = FRENTE;

  attachInterrupt(0, conta, CHANGE);

}

void speedControl()
{
    
    if( changeRef )
    {
      if( ref > 0)
      {
          sentido_rot = FRENTE;
          digitalWrite(b1_A, HIGH);
          digitalWrite(b2_A, LOW);
      }
      else
      {
          sentido_rot = TRAS;
          digitalWrite(b1_A, LOW);
          digitalWrite(b2_A, HIGH);
      }
    }
    
    tempo = tempo + Ta; // [tempo] = ms
    
    velocidade[k] = 1000*((contador[n] - contador[n-1])*(2*PI/PPR))/Ta; // le saida atual
    
    yf = 0.2*velocidade[k]+0.2*velocidade[k-1]+0.2*velocidade[k-2]+0.2*velocidade[k-3]+0.2*velocidade[k-4]; // filtragem
    
    e[n] = ref - yf; //calculo do erro
    
    /*u[n] = u[n-1] - 1.5*e[n-1] + 1.35*e[n]; // calculo sinal de controle
    
    if( u[n] < 0 )
      u[n] = 0;
    else if( u[n] > tensaoMotor )
            u[n] = tensaoMotor; */
            
    if( changeRef )
    {
      u[n] = (ref/tensaoMotor)*255;
      changeRef = 0;
    } 
    else
    {
      if( e[n] > 0 )
      {
        if( u[n] <= 254 )
          u[n] = u[n] + 1;    
      }
      else if( u[n] >= 1 )
              u[n] = u[n] - 1; 
    }
          
        
    analogWrite( EN_A, (int)u[n] ); // atua sobre o sistema  (int)(255*u[n]/tensaoMotor)
      
    Serial.println( (int)(100*yf) );


    // Atualiza as variaveis    
    velocidade[k-4] = velocidade[k-3];
    velocidade[k-3] = velocidade[k-2];
    velocidade[k-2] = velocidade[k-1];
    velocidade[k-1] = velocidade[k];
    e[n-1] = e[n];
    u[n-1] = u[n];
    contador[n-1] = contador[n];
    
}


void conta()
{
 
  contador[n] = contador[n]+1;

  if( DEBUG )
  {
    digitalWrite(13,flag);
    flag = !flag;
  }

}



void loop()
{
  while( Serial.read() != 'b' )
  {  }
  
  delay(100);
  
  MsTimer2::start();
  
  while( true ) //tempo < 22000 )
  {
     /*if( ( tempo == (STEP_TIME*index)*1000 ) && (ref <= tensaoMotor) && !changeRef )
      {
            ref = ref + 2.5;
            changeRef = 1;
            index ++;
      }*/
      
      delay(10);
  }  
  
  /*
  if( Serial.available() )
  {

    while( contador <= numero_voltas*PPR )
    {

      analogWrite( EN_A, motor_A_speed );

      switch( Serial.read() )
      {
      case 'r' :

        Serial.println(contador);

        break; 

      case 'z' :

        contador = 0;

        break;

      case 'v' :

        Serial.println( (int)(velocidade[0]*100000) );
        
        break; 

      case 's' : // Flush Stop motor 

        motor_A_speed = 0;

        break;


      case 't' : // Trava o motor 

        digitalWrite(b1_A, HIGH);
        digitalWrite(b2_A, HIGH);

        break;


      case 'a' : // aumenta velocidade

          if( motor_A_speed <= 240 )
        {
          motor_A_speed = motor_A_speed + 10;
          Serial.println( motor_A_speed );
        }

        break;


      case 'd' : // diminui velocidade

          if( motor_A_speed >= 10 )
        {
          motor_A_speed = motor_A_speed - 10;
          Serial.println( motor_A_speed );
        }

        break;

      case 'i' : // inverte sentido de rotacao do motor

        if( sentido_rot == FRENTE )
        {
          sentido_rot = TRAS;
          digitalWrite(b1_A, LOW);
          digitalWrite(b2_A, HIGH);
        }
        else
        {
          sentido_rot = FRENTE;
          digitalWrite(b1_A, HIGH);
          digitalWrite(b2_A, LOW);
        }

        break;
      }

    }*/
    
    
    MsTimer2::stop();
    Serial.print('s');
    
    analogWrite( EN_A, 255 );
    digitalWrite(b1_A, HIGH);
    digitalWrite(b2_A, HIGH);
    delay(1000);
    analogWrite( EN_A, 0 );
    
     //for( int i=0; i<index+1; i++ )
       //Serial.println( (int)(velocidade[i]*100000) );

  //}

}


